#ifndef MG_GEOMETRY_VECTOR_MATH_H
#define MG_GEOMETRY_VECTOR_MATH_H

#include <boost/geometry/arithmetic/arithmetic.hpp>
#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/strategies/default_length_result.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/select_coordinate_type.hpp>

#include "precision.h"

namespace mg
{
    namespace geometry
    {
        namespace detail
        {
            template<typename Vector1, typename Vector2, std::size_t Dimension, std::size_t DimensionCount>
            struct dot_product_maker
            {
                typedef typename select_coordinate_type_fp<Vector1, Vector2>::type real;

                static inline real apply(const Vector1& p1, const Vector2& p2)
                {
                    real p = static_cast<real>(boost::geometry::get<Dimension>(p1)) * static_cast<real>(boost::geometry::get<Dimension>(p2));
                    real next = dot_product_maker<Vector1, Vector2, Dimension + 1, DimensionCount>::apply(p1, p2);
                    return p + next;
                }
            };

            template<typename Vector1, typename Vector2, std::size_t DimensionCount>
            struct dot_product_maker<Vector1, Vector2, DimensionCount, DimensionCount>
            {
                typedef typename select_coordinate_type_fp<Vector1, Vector2>::type real;

                static inline real apply(const Vector1& p1, const Vector2& p2)
                {
                    real p = static_cast<real>(boost::geometry::get<DimensionCount>(p1)) * static_cast<real>(boost::geometry::get<DimensionCount>(p2));
                    return p;
                }
            };

            template<typename Point1, typename Point2, typename Point3, std::size_t Dimension, std::size_t DimensionCount>
            struct difference_maker
            {
                typedef typename select_coordinate_type_fp<Point1, Point2>::type real;
                typedef typename boost::geometry::coordinate_type<Point3>::type T;

                static inline void apply(const Point1& p1, const Point2& p2, Point3& diff)
                {
                    boost::geometry::set<Dimension>(diff,
                        static_cast<T>(static_cast<real>(boost::geometry::get<Dimension>(p1)) - static_cast<real>(boost::geometry::get<Dimension>(p2))));

                    difference_maker<Point1, Point2, Point3, Dimension + 1, DimensionCount>::apply(p1, p2, diff);
                }
            };

            template<typename Point1, typename Point2, typename Point3, std::size_t DimensionCount>
            struct difference_maker<Point1, Point2, Point3, DimensionCount, DimensionCount>
            {
                typedef typename select_coordinate_type_fp<Point1, Point2>::type real;
                typedef typename boost::geometry::coordinate_type<Point3>::type T;

                static inline void apply(const Point1& p1, const Point2& p2, Point3& diff)
                {
                    boost::geometry::set<DimensionCount>(diff,
                        static_cast<T>(static_cast<real>(boost::geometry::get<DimensionCount>(p1)) - static_cast<real>(boost::geometry::get<DimensionCount>(p2))));
                }
            };
        }

        template<typename Vector1, typename Vector2>
        inline typename select_coordinate_type_fp<Vector1, Vector2>::type
        dot_product(const Vector1& v1, const Vector2& v2)
        {
            BOOST_CONCEPT_ASSERT((boost::geometry::concept::ConstPoint<Vector1>));
            BOOST_CONCEPT_ASSERT((boost::geometry::concept::ConstPoint<Vector2>));

            return detail::dot_product_maker<Vector1, Vector2, 0, boost::geometry::dimension<Vector1>::type::value - 1>::apply(v1, v2);
        }


        template<typename Vector>
        inline typename coordinate_type_fp<Vector>::type length(const Vector& v)
        {
            typedef typename coordinate_type_fp<Vector>::type real;
            return std::sqrt(static_cast<real>(boost::geometry::dot_product(v, v)));
        }

        template<typename Vector>
        inline Vector& normalize(Vector& v)
        {
            typename coordinate_type_fp<Vector>::type norm = length(v);
            if (boost::geometry::math::larger(norm, 0.0))
            { 
                boost::geometry::divide_value(v, 
                    static_cast<boost::geometry::detail::param<Vector>::type>(norm));
            }
            else
            {
                boost::geometry::assign_zero(v);
            }

            return v;
        }

        template<typename Vector>
        Vector unit_vector(const Vector& v)
        {
            typename coordinate_type_fp<Vector>::type norm = length(v);
            if (boost::geometry::math::larger(norm, 0.0))
            {
                Vector unitVector;
                boost::geometry::assign_point(unitVector, v);
                boost::geometry::divide_value(unitVector, static_cast<boost::geometry::detail::param<Vector>::type>(norm));
                return unitVector;
            }
            else
            {
                return boost::geometry::make_zero<Vector>();
            }
        }

        template<typename Point1, typename Point2, typename ReturnType>
        inline ReturnType difference(const Point1& p1, const Point2& p2)
        {
            BOOST_CONCEPT_ASSERT((boost::geometry::concept::ConstPoint<Point1>));
            BOOST_CONCEPT_ASSERT((boost::geometry::concept::ConstPoint<Point2>));
            BOOST_CONCEPT_ASSERT((boost::geometry::concept::ConstPoint<ReturnType>));

            ReturnType diff;
            detail::difference_maker<Point1, Point2, ReturnType, 0, boost::geometry::dimension<Point1>::type::value - 1>::apply(
                p1, p2, diff);
            return diff;
        }

        template<typename Point1, typename Point2>
        inline typename boost::geometry::select_most_precise<Point1, Point2>::type
            difference(const Point1& p1, const Point2& p2)
        {
            return difference<Point1, Point2, boost::geometry::select_most_precise<Point1, Point2>::type>(p1, p2);
        }
    }
}

#endif